/* 
 * File:   Search.cxx
 * Author: enric
 * 
 * Created on 28 de enero de 2012, 13:25
 */

#include "Search.hxx"

Search::Search(std::string filename) {
    this->m_grid_world.load_from(filename);
    
    //std::cout << "Number of actions: " << this->m_grid_world.actions().size() << std::endl;

    //std::cout << "Actions Available: " << std::endl;
    //for( unsigned i = 0; i < this->m_grid_world.actions().size(); i++)
    //        std::cout << "\t"<< this->m_grid_world.actions()[ i ]->identifier() << " - " <<this->m_grid_world.actions()[ i ]->to_string() << std::endl;


    //std::cout << "Applicable Actions in the initial state: " << std::endl;
    
    this->m_grid_world.avaiableActions( this->m_grid_world.initial_state() );
    this->m_open.push_back(new Node(this->m_grid_world.initial_state(), this->m_grid_world.actions()[0]));
    this->m_open[0]->setFather(*this->m_open[0]);
}

float Search::get_eval(Node& node)
{
    return this->get_heuristics(*node.state()) + this->get_acom(node);
}

/*
 * Retorna la distància manhattan, en aquest cas, la suma de la distància de les
 * claus al goal.
 */
int Search::get_heuristics(State& state)
{
    int sum = 0;
    for ( unsigned k = 0; k < state.keys().size(); k++ )
    {
	sum += this->distance(state.keys()[k]->location()->x(), this->m_grid_world.final_state().keys()[k]->location()->x(), state.keys()[k]->location()->y(), this->m_grid_world.final_state().keys()[k]->location()->y());
    }
    sum += this->distance(state.agent()->location()->x(), this->m_grid_world.final_state().agent()->location()->x(), state.agent()->location()->y(), this->m_grid_world.final_state().agent()->location()->y());
    return sum;
}

int Search::get_acom(Node& node)
{
    //int value = this->get_acom(state, 0);
    //std::cout << "Valor final: " << value << std::endl;
    
    if((*node.state() == this->m_grid_world.initial_state()) && (*node.father()->state() == *node.state())) {
        return 0;
    }
    else {
        return get_acom(*node.father()) + 1;
    }
}

void Search::addPath(Node& node) {
    if((*node.state() == this->m_grid_world.initial_state()) && (*node.father()->state() == *node.state())) {
        return;
    }
    else {
        //std::cout << "Node: " << std::endl << node.state()->to_string() << std::endl;
        this->m_path.push_back(new Node(*node.state(), node.action()));
        this->addPath(*node.father());
    }    
}

/* 
 * Si es crida la funció ida_star sense cap argument, s'encarrega de cridar
 * ida_star(State& init_state) utilitzant com a estat l'inicial del grid.
 */
void Search::a_star()
{
    int boundary = this->get_heuristics(*this->m_open[0]->state());
    this->a_star(*this->m_open[0], boundary);
}

/*
 * ida_star ens mostra l'estat amb el que és cridat, seguidament mira si l'estat
 * amb el que és cridat és l'estat objectiu. Si ho és surt de la funció, si no,
 * ens ensenya les accions a triar des d'aquell estat i n'escull una en funció
 * de l'heurística.
 */
void Search::a_star(Node& init_node, int boundary)
{   
//    std::cout << "Actual state: " << std::endl;
//    std::cout << init_node.state()->to_string() << std::endl;
    
    if(*init_node.state() == this->m_grid_world.final_state())
    {
        this->addPath(init_node);
        this->m_values = this->m_path.size();
    }
    else
    {
 //       std::cout << "Applicable Actions in the second state: " << std::endl;
                        
        //En l'estat next guardem l'estat al que anirem a parar despres d'haver
        //triat una accio
        this->expand_node(init_node);
        this->evaluateOpen();
        Node next = this->getBestFromOpen();
 //       bool pruning = this->pruning(boundary);
        
 //       if(pruning) {
 //           this->m_closed.clear();
 //           this->m_open.clear();
 //           this->m_open.push_back(new Node(this->m_grid_world.initial_state()));
 //           this->m_open[0]->setFather(*this->m_open[0]);
 //           this->ida_star(*this->m_open[0], next.eval());
 //       }
 //       else {
            //Tornem a cridar la funció amb el mateix boundary.
            this->a_star(next, boundary);
 //       }
        //delete &next;
    }
}

bool Search::pruning(int boundary) {
    //std::cout << "mida de l'open quan entrem al pruning: " << this->m_open.size() << std::endl;
    //std::cout << "valor del boundary: " << boundary << std::endl;
    for(unsigned int i = 0; i < this->m_open.size(); i++) {
        //std::cout << "eval del node " << i << ": " << this->m_open[i]->eval() << std::endl;
        if(boundary < this->m_open[i]->eval()) {
            this->eraseElement(this->m_open, *this->m_open[i]);
            i--;
        }
    }
    if(this->m_open.empty())
    {
        //std::cout << "S'ha buidat tot" << std::endl;
        return true;
    }
    else
        return false;
}

bool Search::isNodeInClosed(Node& node) {
    for(unsigned int i = 0; i < this->m_closed.size(); i++) {
        if(this->m_closed[i]->state()->to_string() == node.state()->to_string())
            return true;
    }
    return false;
}

void Search::eraseElement(std::vector<Node*>& nodes, Node& node) {
    for (std::vector<Node*>::iterator it = nodes.begin(); it <= nodes.end(); ++it) {
        if((*it)->state()->to_string() == node.state()->to_string()) {
            if(!nodes.empty())
                nodes.erase(it);
        }
    }
}

void Search::expand_node(Node& node) {
    //Mirar si el nodo esta en el open
    
    //if(!this->isNodeInClosed(node)) {
        this->eraseElement(this->m_open, node);
        this->m_closed.push_back(&node);

        //Aquestes son les accions possibles desde l'estat actual
        std::vector<Action*> actions = this->m_grid_world.avaiableActions(*node.state());

        for(unsigned int i = 0; i < actions.size(); i++) {
            Node* iter = new Node(*(actions[i])->execute(node.state()), actions[i]);
            //ara, l'estat al que anirem sap que el seu pare es next
            iter->setFather(node);

            //Guardem a la hash l'accio que ens ha portat al nou estat
    //        this->m_action_state.insert(std::make_pair<Action*, State*>(actions[i], &iter));
    //        this->m_state_action.insert(std::make_pair<State*, Action*>(&iter, actions[i]));

            this->m_open.push_back(iter);
            //std::cout << "Nodes: " << this->m_open.size() << std::endl;
            //std::cout << iter->state()->to_string() << std::endl;

            //delete &iter;
        }
    //}
}

void Search::evaluateOpen() {
    for(unsigned int i = 0; i < this->m_open.size(); i++) {
        if(!this->m_open[i]->isEvaluated()) {
            this->m_open[i]->setHeuristics(this->get_heuristics(*this->m_open[i]->state()));
            this->m_open[i]->setAcom(this->get_acom(*this->m_open[i]));
            this->m_open[i]->setEval();
        }
    }
}

Node& Search::getBestFromOpen() {
    int pos = 0;
    //std::cout << "La primera merda de l'open: " << std::endl << this->m_open[0]->to_string() << std::endl;
    int min_eval = this->m_open[0]->eval();
    //std::cout << "Cost del node 0: " << this->m_open[0]->eval() << " = " << this->m_open[0]->heuristics() << " + " << this->m_open[0]->acom() << std::endl;
    for(unsigned int i = 1; i < this->m_open.size(); i++) {
        //std::cout << "Cost del node " << i << ": " << this->m_open[i]->eval() << " = " << this->m_open[i]->heuristics() << " + " << this->m_open[i]->acom() << std::endl;
        if(this->m_open[i]->eval() < min_eval) {
            min_eval = this->m_open[i]->eval();
            pos = i;
        }
    }
    //std::cout << "Min eval: " << min_eval << std::endl;
    
    return *this->m_open[pos];
}

int Search::distance(unsigned int start_x, unsigned int final_x, unsigned int start_y, unsigned int final_y) {
    return std::abs((int)(final_x - start_x)) + std::abs((int)(final_y - start_y));
}

void Search::writeValues() {
    std::ofstream fp_out("solution.dat");
    for(int i = this->path().size() - 1; i >= 0; i--) {
        fp_out << this->path()[i]->action()->identifier() << std::endl;
    }
    fp_out.close();
}